#include "Prm.h"

Prm::Prm (	int number_vertices, 
	int k_nearest, 
	CollisionDetector* col, 
	Sampler* sampler,
	Point_2 start, Point_2 target) : 
m_number_vertices(number_vertices), m_k_nearest(k_nearest), 
	m_col(col), m_sampler(sampler),m_graph(0),
	m_start(start), m_target(target)
{
	m_graph = new Graph<Point_2,Less_than_int>(0,true);
	m_kd_tree = new Kd_tree_2<Kernel>(m_number_vertices);
}

Prm::~Prm()
{
	delete m_graph;
	delete m_kd_tree;
}

void Prm::generate_roadmap()
{
	for (unsigned int i = 0; i < m_number_vertices; ++i)
	{
		m_kd_tree->insert(m_sampler->generate_sample());
	}

	vector<Point_2> all_tree_points;

	m_kd_tree->points(std::back_inserter(all_tree_points));

	for (vector<Point_2>::iterator current_point_it = all_tree_points.begin();
		current_point_it != all_tree_points.end();
		current_point_it++)
	{
			add_point_to_graph(*current_point_it);
	}
}

void Prm::add_point_to_graph(Point_2 p)
{
	vector<Point_2> k_nearest_neighbors;

	m_kd_tree->k_nearest_neighbors(p, m_k_nearest, std::back_inserter(k_nearest_neighbors));

	for (vector<Point_2>::iterator k_nearest_neighbors_it = k_nearest_neighbors.begin();
		k_nearest_neighbors_it != k_nearest_neighbors.end();
		k_nearest_neighbors_it++)
	{
		if (m_col->local_planner(p, *k_nearest_neighbors_it, (m_col->m_radius / 2)));
		{
			m_graph->add_vertex(p);
			m_graph->add_vertex(*k_nearest_neighbors_it);
			m_graph->add_edge(p, *k_nearest_neighbors_it);
		}
	}
}

vector<Point_2> Prm::retrieve_path()
{
	std::list<Point_2> path;
	add_point_to_graph(m_start);
	add_point_to_graph(m_target);
//	m_graph->print_connected_components();
	m_graph->find_path(m_start,m_target,path);
	return vector<Point_2>(path.begin(),path.end());
}
